%% 计算车辆四个方向与障碍物之间的距离的二范数
function [frontDistance, backDistance, leftDistance, rightDistance] = VehicleObstDistance(pos,theta,Vehicle,Configure)
    % 车辆尺寸
    W = Vehicle.W;
    LF = Vehicle.LF;
    LB = Vehicle.LB;

    % 四个角的位置
    Cornerfl = [LF, W/2];
    Cornerfr = [LF, -W/2];
    Cornerrl = [-LB, W/2];
    Cornerrr = [-LB, -W/2];

    % 方向余弦矩阵，负号是 把坐标转换为基坐标
    dcm = angle2dcm(-theta, 0, 0);
    
    % 获取四个角的全局坐标
    tvec = dcm*[Cornerfl';0]; % 旋转变换
    tvec = tvec';
    Cornerfl = tvec(1:2)+pos; % 平移变换
    
    tvec = dcm*[Cornerfr';0];
    tvec = tvec';
    Cornerfr = tvec(1:2)+pos;
    
    tvec = dcm*[Cornerrl';0];
    tvec = tvec';
    Cornerrl = tvec(1:2)+pos;
    
    tvec = dcm*[Cornerrr';0];
    tvec = tvec';
    Cornerrr = tvec(1:2)+pos;

    % 记录构成车辆模型的四条直线的起止坐标
    Rect = []; %#ok<*NASGU> 
    Rect(end+1,:) = [Cornerfl, Cornerfr]; % 前
    Rect(end+1,:) = [Cornerfr, Cornerrr]; % 右
    Rect(end+1,:) = [Cornerrr, Cornerrl]; % 后
    Rect(end+1,:) = [Cornerrl, Cornerfl]; % 左

    % 获取障碍物
    ObstList = Configure.OBSTACLE_LIST;

    % 计算到车辆四个方向的最近障碍物的距离
    [frontK,frontB] = CalculateLine(Cornerfl,Cornerfr);
    frontDistance = VehicleObstDistance1(frontK,frontB,Cornerrr,ObstList);
    [rightK,rightB] = CalculateLine(Cornerfr,Cornerrr);
    rightDistance = VehicleObstDistance1(rightK,rightB,Cornerrl,ObstList);
    [backK,backB] = CalculateLine(Cornerrr,Cornerrl);
    backDistance = VehicleObstDistance1(backK,backB,Cornerfl,ObstList);
    [leftK,leftB] = CalculateLine(Cornerrl,Cornerfl);
    leftDistance = VehicleObstDistance1(leftK,leftB,Cornerfr,ObstList);
end

function [k,b] = CalculateLine(point1,point2)
    k = (point2(2)-point1(2)) / (point2(1)-point1(1));
    b = point2(2) - k*point2(1);
end

function distance = VehicleObstDistance1(k,b,oppoPoint,ObstList)
    distance = inf;
    if k*oppoPoint(1)+b > oppoPoint(2)  % 反方向点在直线上方
        for i = 1:length(ObstList)
            x = ObstList(i,1);
            y = ObstList(i,2);
            if k*x+b < y  % 障碍物在直线下方
                d = abs(k*x-y+b)/sqrt(k^2+1);
                if d < distance
                    distance = d;
                end
            end
        end
    else  % 反方向点在直线下方
        for i = 1:length(ObstList)
            x = ObstList(i,1);
            y = ObstList(i,2);
            if k*x+b > y  % 障碍物在直线上方
                d = abs(k*x-y+b)/sqrt(k^2+1);
                if d < distance
                    distance = d;
                end
            end
        end
    end
end